#include <lcm/lcm-cpp.hpp>
#include "vehicles.hpp"
#include "EnvironmentData.hpp"
#include "tinyxml2.h"

class Handler
{

  public:
    VehicleList veh_list;
    float self_x;
    float self_y;
    float self_z;

    ~Handler() {}

    void handlerMessage(const lcm::ReceiveBuffer *rbuf,
                        const std::string &chan,
                        const EnvironmentData *msg)
    {
        // std::cout << "Speed: " << msg->velocity << std::endl;
        // std::cout << "Heading: " << msg->heading << std::endl;
        // std::cout << "Acceleration: " << msg->acceleration << std::endl;
        std::cout << "Vehs_num: " << msg->vehs_num << std::endl;
        this->veh_list.ClearVehList();

        this->veh_list.VisualizerClear();

        this->veh_list.AddVehicle(0, 0, 0, msg->bbox.x, msg->bbox.y, msg->bbox.z, msg->heading+90);
        
        self_x = msg->pos.x;
        self_y = msg->pos.y;
        self_z = msg->pos.z;

        for (int i = 0; i < msg->vehs_num; i++)
        {
            float pos_x =  msg->vehicles[i].pos.x - msg->pos.x;
            float pos_y = msg->vehicles[i].pos.y - msg->pos.y;

            veh_list.AddVehicle(pos_x, pos_y, 0,
                                msg->vehicles[i].bbox.x,
                                msg->vehicles[i].bbox.y,
                                msg->vehicles[i].bbox.z,
                                msg->vehicles[i].heading+90,
                                1, 0, 0);
        }
    }
};

int main(int argc, char **argv)
{

    ros::init(argc, argv, "Visualization");

    ros::NodeHandle n;

    ros::Rate r(30);

    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("traffic_report", 10);

    lcm::LCM lcm("udpm://239.255.76.67:7667?ttl=1");
    if (!lcm.good())
        return 1;
    Handler handler_object;
    lcm.subscribe("GetEnvironment", &Handler::handlerMessage, &handler_object);


    const char *xml_path = "/home/robotor/workspace/ros_ws/CreatMap/src/draw_map/res/Path.xml";

    tinyxml2::XMLDocument doc;
    if (doc.LoadFile(xml_path) != 0)
    {

        std::cout << xml_path << std::endl;
        std::cout << "Load xml file failed!" << std::endl;
        return -1;
    }

    tinyxml2::XMLElement *root = doc.RootElement();
    tinyxml2::XMLElement *path_root = root->FirstChildElement("Path");



    while (ros::ok())
    {
        lcm.handle();

        while (marker_pub.getNumSubscribers() < 1)
        {
            if (!ros::ok())
            {
                return 0;
            }

            ROS_WARN_ONCE("Please create a subscriber to the marker");
            sleep(1);
        }

        ROS_INFO("Vehs size %d !", handler_object.veh_list.GetSize());


        /**** Draw Path Curbstone  Strat...***/

        visualization_msgs::Marker left_lines, right_lines, points;  //points is center poinst(green points)
        points.header.frame_id = left_lines.header.frame_id = right_lines.header.frame_id = "/test_frame";
        points.header.stamp = left_lines.header.stamp = right_lines.header.stamp = ros::Time::now();

        points.ns = left_lines.ns = right_lines.ns = "Path_Curbstone";
        points.action = left_lines.action = right_lines.action = visualization_msgs::Marker::ADD;

        points.pose.orientation.w = left_lines.pose.orientation.w = right_lines.pose.orientation.w = 1.0;

        left_lines.id = 0;
        right_lines.id = 1;
        points.id = 2;

        left_lines.type = right_lines.type = visualization_msgs::Marker::LINE_STRIP;
        points.type = visualization_msgs::Marker::POINTS;

        points.scale.x = 0.3;
        points.scale.y = 0.3;

        left_lines.scale.x = right_lines.scale.x = 0.5;

        points.color.g = 1.0f;
        points.color.a = 0.5;

        left_lines.color.r = right_lines.color.r = 0;
        left_lines.color.g = right_lines.color.g = 0;
        left_lines.color.b = right_lines.color.b = 0;

        left_lines.color.a = right_lines.color.a = 1.0;

        tinyxml2::XMLElement *path_node = path_root->FirstChildElement("Node");

        geometry_msgs::Point origin_p;

        origin_p.x = handler_object.self_x;
        origin_p.y = handler_object.self_y;
        origin_p.z = handler_object.self_z;

        while (path_node != NULL)
        {

            geometry_msgs::Point l_p, r_p, c_p;

            c_p.x = atof(path_node->Attribute("PositionX")) - origin_p.x;
            c_p.y = atof(path_node->Attribute("PositionY")) - origin_p.y;
            c_p.z = atof(path_node->Attribute("PositionZ")) - origin_p.z;

            l_p.x = atof(path_node->Attribute("EdgeNode1X")) - origin_p.x;
            l_p.y = atof(path_node->Attribute("EdgeNode1Y")) - origin_p.y;
            l_p.z = atof(path_node->Attribute("EdgeNode1Z")) - origin_p.z;

            r_p.x = atof(path_node->Attribute("EdgeNode2X")) - origin_p.x;
            r_p.y = atof(path_node->Attribute("EdgeNode2Y")) - origin_p.y;
            r_p.z = atof(path_node->Attribute("EdgeNode2Z")) - origin_p.z;


            points.points.push_back(c_p);
            left_lines.points.push_back(l_p);
            right_lines.points.push_back(r_p);

            path_node = path_node->NextSiblingElement();
        }

        marker_pub.publish(points);
        marker_pub.publish(left_lines);
        marker_pub.publish(right_lines);

        /**** Draw Path Curbstone  Done...***/

        for (int i = 0; i < handler_object.veh_list._vehs.size(); i++)
        {

            marker_pub.publish(handler_object.veh_list._vehs[i]);
        }
        r.sleep();
    }
}